﻿using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using System;
using System.Drawing;

namespace FinalYear.OpticalFlow
{
    public class FocusofExpansion
    {
        private static int i = 0;

        public static void Calculate(PointF[] _leftPoints, PointF[] _rightPoints)
        {
            //if (i > 3)
            //{
            //    int _pointCount = _leftPoints.Length;
            //    IntPtr points1 = CreatePointListPointer(_leftPoints);
            //    IntPtr points2 = CreatePointListPointer(_rightPoints);
            //    IntPtr status = CvInvoke.cvCreateMat(1, _pointCount, MAT_DEPTH.CV_8U);

            //    IntPtr fundamentalMatrix = CvInvoke.cvCreateMat(3, 3, MAT_DEPTH.CV_32F);
            //    IntPtr epiLines = CvInvoke.cvCreateMat(3, 3, MAT_DEPTH.CV_32F);
            //    CvInvoke.cvFindFundamentalMat(points1, points2, fundamentalMatrix, Emgu.CV.CvEnum.CV_FM.CV_FM_RANSAC, 1.0, 0.99, IntPtr.Zero);
            //    CvInvoke.cvComputeCorrespondEpilines(points1, 1, fundamentalMatrix, epiLines);

            //}

            //i++;
        }

        public static IntPtr CreatePointListPointer(PointF[] points)
        {
            int _pointCount = points.Length;
            IntPtr result = CvInvoke.cvCreateMat(_pointCount, 2, MAT_DEPTH.CV_32F);

            for (int i = 0; i < _pointCount; i++)
            {
                double currentX = points[i].X;
                double currentY = points[i].Y;
                CvInvoke.cvSet2D(result, i, 0, new MCvScalar(currentX));
                CvInvoke.cvSet2D(result, i, 1, new MCvScalar(currentY));
            }

            return result;
        }
    }
}